﻿#include "PCLFit.h"
#include <vtkGenericOpenGLRenderWindow.h>
#include <vtkAxesActor.h>
#include <vtkOBJReader.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/vtk_io.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h> 
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/surface/poisson.h>
#include <pcl/surface/mls.h>
#include <pcl/surface/marching_cubes_hoppe.h>
#include <pcl/surface/marching_cubes_rbf.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/common/transforms.h>

#include <vtkPlaneSource.h>
#include <vtkPolyData.h>
#include <vtkActor.h>
#include <vtkCylinderSource.h>
#include <vtkTubeFilter.h>
PCLFit::PCLFit(QWidget* parent)
	: QMainWindow(parent)
{
	vtkWidget = new QVTKOpenGLNativeWidget(this);
	this->resize(600, 400);
	this->setCentralWidget(vtkWidget);

	planeCloud.reset(new pcl::PointCloud<pcl::PointXYZ>);

	for (size_t i = 0; i < 100; i++)
	{
		for (size_t j = 0; j < 100; j++)
		{
			planeCloud->push_back({ i * 0.01f, j * 0.01f, rand() % 100 * 0.001f });
		}
	}

	auto renderWindow = vtkSmartPointer<vtkGenericOpenGLRenderWindow>::New();
	auto renderer = vtkSmartPointer<vtkRenderer>::New();
	renderWindow->AddRenderer(renderer);
	// PCL 点云显示到VTK
	viewer = pcl::visualization::PCLVisualizer::Ptr(new pcl::visualization::PCLVisualizer(
		renderer
		, renderWindow, "", false));
	// 可视化拟合结果
	viewer->addPointCloud<pcl::PointXYZ>(planeCloud, "plane"); //添加平面点云
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "plane"); //颜色
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "plane"); //点的大小

	// 拟合平面
	auto&& cloud_plane = FitPlane(planeCloud);
	// 显示平面
	vtkNew<vtkPlaneSource> planeSource;
	planeSource->SetCenter(0.5, 0.5, std::get<3>(cloud_plane));
	planeSource->SetNormal(std::get<0>(cloud_plane), std::get<1>(cloud_plane), std::get<2>(cloud_plane));
	planeSource->Update();
	vtkNew<vtkPolyDataMapper> planeMapper;
	planeMapper->SetInputConnection(planeSource->GetOutputPort());
	vtkNew<vtkActor> planeActor;
	planeActor->SetMapper(planeMapper);
	renderer->AddActor(planeActor);

	vtkNew<vtkAxesActor> axes;
	renderer->AddActor(axes);


	// 拟合圆柱体
	// 读取PCB
	cylinderCloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile("D:/TEST/3D/pcb/cylinder.pcd", *cylinderCloud);
	
	// 点云缩放
	// cylinderCloud乘系数
	//pcl::transformPointCloud(*cylinderCloud,
	//	*cylinderCloud,
	//	Eigen::Affine3f(Eigen::Scaling(.1f)));

	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> o_color(cylinderCloud, 0, 0, 255);
	viewer->addPointCloud<pcl::PointXYZ>(cylinderCloud, o_color, "o_point_cloud");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "o_point_cloud");


	auto&& cylinder = FitCylinder(cylinderCloud);
	auto&& cloud_cylinder = std::get<3>(cylinder);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> set_color(cloud_cylinder, 0, 255, 0);
	viewer->addPointCloud<pcl::PointXYZ>(cloud_cylinder, set_color, "segment_point_cloud");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "segment_point_cloud");

	// 获取圆柱点云的高度


	// 圆柱体法向量一点
	auto&& normal_point = std::get<0>(cylinder);
	// 圆柱体轴线法向量
	auto&& normal = std::get<1>(cylinder);
	// 圆柱体半径
	auto&& radius = std::get<2>(cylinder);

	// 点投影到直线的位置
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projection(new pcl::PointCloud<pcl::PointXYZ>);
	// 遍历cloud_cylinder
	for (auto&& point : *cloud_cylinder)
	{
		// 点投影到直线
		double t = -(
			normal.x * (normal_point.x - point.x) +
			normal.y * (normal_point.y - point.y) +
			normal.z * (normal_point.z - point.z)) /
			(normal.x * normal.x + normal.y * normal.y + normal.z * normal.z);
		// 点投影到直线的位置
		cloud_projection->push_back({
			(float)(normal_point.x + normal.x * t),
			(float)(normal_point.y + normal.y * t),
			(float)(normal_point.z + normal.z * t)
			});
	}


	// 旋转到X轴	
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_rotation(new pcl::PointCloud<pcl::PointXYZ>);
	// 获取normal绕Z轴角度
	double angle_z = atan2(normal.y, normal.x);
	// 获取normal需要绕Y旋转角度
	double angle_y = atan2(normal.z, normal.x);
	// 绕Z轴旋转
	pcl::transformPointCloud(*cloud_projection,
		*cloud_rotation,
		Eigen::Affine3f(Eigen::AngleAxisf(angle_z, Eigen::Vector3f::UnitZ())));
	// 绕Y轴旋转
	pcl::transformPointCloud(*cloud_rotation,
		*cloud_rotation,
		Eigen::Affine3f(Eigen::AngleAxisf(angle_y, Eigen::Vector3f::UnitY())));

	// 获取X最大值索引
	auto&& max_x_point = std::max_element(cloud_rotation->begin(), cloud_rotation->end(), [](const pcl::PointXYZ& a, const pcl::PointXYZ& b) {
		return a.x < b.x;
		}).operator->();
		//qDebug输出点
		qDebug() << "max_x_point: " << max_x_point->x << " " << max_x_point->y << " " << max_x_point->z;

		// 获取X最小值索引
		auto&& min_x_point = std::min_element(cloud_rotation->begin(), cloud_rotation->end(), [](const pcl::PointXYZ& a, const pcl::PointXYZ& b) {
			return a.x < b.x;
			}).operator->();
			//qDebug输出点
			qDebug() << "min_x_point: " << min_x_point->x << " " << min_x_point->y << " " << min_x_point->z;

			// 结果点云
			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_result(new pcl::PointCloud<pcl::PointXYZ>);
			cloud_result->push_back(*max_x_point);
			cloud_result->push_back(*min_x_point);

			// 反旋转
			pcl::transformPointCloud(*cloud_result,
				*cloud_result,
				Eigen::Affine3f(Eigen::AngleAxisf(-angle_y, Eigen::Vector3f::UnitY())));
			pcl::transformPointCloud(*cloud_result, *cloud_result,
				Eigen::Affine3f(Eigen::AngleAxisf(-angle_z, Eigen::Vector3f::UnitZ())));

			auto&& end_point = cloud_result->at(0);
			auto&& start_point = cloud_result->at(1);
			// 距离
			auto&& distance = sqrt(
				pow(start_point.x - end_point.x, 2) +
				pow(start_point.y - end_point.y, 2) +
				pow(start_point.z - end_point.z, 2)
			);

			// 获取start_point，end_point中心点
			auto&& center_point = pcl::PointXYZ{ (start_point.x + end_point.x) / 2, (start_point.y + end_point.y) / 2, (start_point.z + end_point.z) / 2 };

			// 显示圆柱体

			vtkSmartPointer<vtkLineSource> lineSource =
				vtkSmartPointer<vtkLineSource>::New();
			lineSource->SetPoint1(start_point.x, start_point.y, start_point.z);
			lineSource->SetPoint2(end_point.x, end_point.y, end_point.z);
			vtkSmartPointer<vtkTubeFilter> tubeFilter = vtkSmartPointer<vtkTubeFilter>::New();
			tubeFilter->SetInputConnection(lineSource->GetOutputPort());
			tubeFilter->SetRadius(radius);
			tubeFilter->SetNumberOfSides(50);
			tubeFilter->CappingOn();
			tubeFilter->Update();
			vtkSmartPointer<vtkPolyData> polydata = tubeFilter->GetOutput();
			vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
			mapper->SetInputData(polydata);
			vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
			actor->SetMapper(mapper);
			actor->GetProperty()->SetColor(255, 0, 0);
			renderer->AddActor(actor);

			// VTK 点云显示到VTK
			vtkWidget->setRenderWindow(viewer->getRenderWindow());
			viewer->setupInteractor(vtkWidget->interactor(), vtkWidget->renderWindow());
}

PCLFit::~PCLFit()
{}

std::tuple<double, double, double, double> PCLFit::FitPlane(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
	//选择拟合点云与几何模型
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);

	//创建随机采样一致性对象
	pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));

	//设置距离阈值
	pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);
	ransac.setDistanceThreshold(0.01);

	//执行模型估计
	ransac.computeModel();

	// 根据索引提取内点 
	///方法1
	//std::vector<int> inliers; //存储内点索引的向量
	//ransac.getInliers(inliers); //提取内点对应的索引
	//pcl::copyPointCloud(*cloud, inliers, *cloud_plane);

	///方法2,需要pcl/filters/extract_indices.h头文件，较为繁琐。
	//pcl::PointIndices pi;
	//ransac.getInliers(pi.indices);
	//pcl::IndicesPtr index_ptr(new vector<int>(pi.indices));/// 将自定义的点云索引数组pi进行智能指针的转换
	//pcl::ExtractIndices<pcl::PointXYZ> extract;
	//extract.setInputCloud(cloud);
	//extract.setIndices(index_ptr);
	//extract.setNegative(false); /// 提取索引外的点云，若设置为true,则与copyPointCloud提取结果相同
	//extract.filter(*cloud_plane);
	//= 根据索引提取内点 =

	// 输出模型参数Ax+By+Cz+D=0
	Eigen::VectorXf coefficient;
	ransac.getModelCoefficients(coefficient);
	qDebug() << "平面方程为：\n"
		<< coefficient[0] << "x + "
		<< coefficient[1] << "y + "
		<< coefficient[2] << "z + "
		<< coefficient[3] << " = 0";
	return { coefficient[0] ,coefficient[1],coefficient[2],coefficient[3] };
}

std::tuple<pcl::PointXYZ, pcl::PointXYZ,
	double, pcl::PointCloud<pcl::PointXYZ>::Ptr>
	PCLFit::FitCylinder(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
	// 创建法向量估计对象
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
	// 设置搜索方式
	ne.setSearchMethod(tree);
	// 设置输入点云
	ne.setInputCloud(cloud);
	// 设置K近邻搜索点的个数
	ne.setKSearch(50);
	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
	// 计算法向量，并将结果保存到cloud_normals中
	ne.compute(*cloud_normals);

	//圆柱体分割
	// 创建圆柱体分割对象
	pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
	// 设置输入点云：待分割点云 
	seg.setInputCloud(cloud);
	// 设置对估计的模型系数需要进行优化
	seg.setOptimizeCoefficients(true);
	// 设置分割模型为圆柱体模型
	seg.setModelType(pcl::SACMODEL_CYLINDER);
	// 设置采用RANSAC算法进行参数估计
	seg.setMethodType(pcl::SAC_RANSAC);
	// 设置表面法线权重系数
	seg.setNormalDistanceWeight(0.1);
	// 设置迭代的最大次数
	seg.setMaxIterations(10000);
	// 设置内点到模型距离的最大值
	seg.setDistanceThreshold(0.05);
	// 设置圆柱模型半径的范围
	seg.setRadiusLimits(3.0, 4.0);
	// 设置输入法向量
	seg.setInputNormals(cloud_normals);
	// 保存分割结果
	pcl::PointIndices::Ptr inliers_cylinder(new pcl::PointIndices);
	// 保存圆柱体模型系数
	pcl::ModelCoefficients::Ptr coefficients_cylinder(new pcl::ModelCoefficients);
	// 执行分割，将分割结果的索引保存到inliers_cylinder中，同时存储模型系数coefficients_cylinder
	seg.segment(*inliers_cylinder, *coefficients_cylinder);
	qDebug() << "\n\t\t -圆柱体系数 -";
	qDebug() << "轴线一点坐标：(" <<
		coefficients_cylinder->values[0] << ", "
		<< coefficients_cylinder->values[1] << ", "
		<< coefficients_cylinder->values[2] << ")";

	qDebug() << "轴线方向向量：(" <<
		coefficients_cylinder->values[3] << ", "
		<< coefficients_cylinder->values[4] << ", "
		<< coefficients_cylinder->values[5] << ")";

	qDebug() << "圆柱体半径：" << coefficients_cylinder->values[6];

	//提取分割结果
	// 创建索引提取点对象
	pcl::ExtractIndices<pcl::PointXYZ> extract;
	// 设置输入点云：待分割点云
	extract.setInputCloud(cloud);
	// 设置内点索引
	extract.setIndices(inliers_cylinder);
	// 默认false，提取圆柱体内点；true，提取圆柱体外点
	extract.setNegative(false);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cylinder(new pcl::PointCloud<pcl::PointXYZ>());
	// 执行滤波，并将结果点云保存到cloud_cylinder中
	extract.filter(*cloud_cylinder);
	return {
		{
			coefficients_cylinder->values[0],
			coefficients_cylinder->values[1],
			coefficients_cylinder->values[2]
		},
		{
			coefficients_cylinder->values[3],
			coefficients_cylinder->values[4],
			coefficients_cylinder->values[5]
		},
		coefficients_cylinder->values[6],
		cloud_cylinder };
}
